# 通过占用栅格化地图以及无人机初始位置、目标点位置，使用A*或者其他算法实现路径规划
# 并且发布路径，让contorl_node按照预计路径进行控制

import rospy
from nav_msgs.msg import OccupancyGrid
import get_info_from_gz as gzinfo
import numpy as np
import threading
import time
import math
import map_reading as mr

class AStar():
    """
    描述:A*算法实现
    参数:
        start:list [x,y] 起点
        target:list [x,y] 终点
        map:OccupancyGrid 地图
    """
    def __init__(self, start, target, map):
        self.start = start
        self.target = target
        self.map = map
        # open_list: [[x,y,f,g,h,(parent)]]
        self.open_list = []
        self.close_list = []
        self.path = []
        self.open_list.append([self.start[0],self.start[1],0,0,0,(-1,-1)])
        self.step = 0
    """
    描述:判断点是否在地图内
    参数:
        point:list [x,y] 点
    返回值:
        bool
    """
    def is_in_map(self, point):
        if point[0] < 0 or point[0] >= self.map.info.width or point[1] < 0 or point[1] >= self.map.info.height:
            return False
        else:
            return True
    """
    描述:判断点是否在障碍物内,未知状态的点也当作障碍物
    参数:
        point:list [x,y] 点
    返回值:
        bool
    """
    def is_in_obstacle(self, point):
        # print(len(self.map.data))
        # print(point[1]*self.map.info.width+point[0])
        if self.map.data[point[1]*self.map.info.width+point[0]] == 100 or self.map.data[point[1]*self.map.info.width+point[0]] == -1:
            return True
        else:
            return False
    """
    描述:判断点是否在close_list内
    参数:
        point:list [x,y] 点
    返回值:
        bool
    """ 
    def is_in_close_list(self, point):
        for i in range(len(self.close_list)):
            if point == self.close_list[i]:
                return True
        return False
    """
    描述:判断点是否在open_list内
    参数:
        point:list [x,y] 点
    返回值:
        bool
    """
    def is_in_open_list(self, point):   
        for i in range(len(self.open_list)):
            if point == self.open_list[i]:
                return True
        return False
    """
    描述:计算当前节点到终点的欧几里得距离作为启发式函数值
    参数:
        current_point:list [x,y] 当前节点
    返回值:
        float 启发式函数值
    """
    def heuristic_Euclid(self, current_point):
        return math.sqrt((self.target[0]-current_point[0])**2+(self.target[1]-current_point[1])**2)

    """
    描述:计算当前节点到终点的曼哈顿距离作为启发式函数值
    参数:
        current_point:list [x,y] 当前节点
    返回值:
        float 启发式函数值
    """
    def heuristic_Manhattan(self, current_point):
        return abs(self.target[0]-current_point[0])+abs(self.target[1]-current_point[1])
    
    """
    描述:计算当前节点到终点的切比雪夫距离作为启发式函数值
    参数:
        current_point:list [x,y] 当前节点
    返回值:
        float 启发式函数值
    """ 
    def heuristic_Chebyshev(self, current_point):
        return max(abs(self.target[0]-current_point[0]), abs(self.target[1]-current_point[1]))
    
    """
    描述:计算当前节点到终点的对角距离作为启发式函数值
    参数:
        current_point:list [x,y] 当前节点
    返回值:
        float 启发式函数值
    """
    def heuristic_Diagonal(self, current_point):
        dx = abs(self.target[0]-current_point[0])
        dy = abs(self.target[1]-current_point[1])
        return (dx+dy)+(math.sqrt(2)-2)*min(dx, dy)
    
    """
    描述:在openlist中找到f值最小的节点
    参数:
        无
    返回值:
        list [x,y,f,g,h,(parent)] f值最小的节点
    """
    def find_min_f_point(self):
        min_f = self.open_list[0]
        for i in range(len(self.open_list)):
            # print(self.open_list[i])
            if self.open_list[i][2] < min_f[2]:
                min_f = self.open_list[i]
        return min_f
    
    """
    描述:使用Astar计算出路径
    参数:
        heuristic_function:func 启发式函数
    返回值:
        list 路径
    """
    def get_path(self, func):
        while len(self.open_list) > 0:
            self.step += 1
            print('step:', self.step)
            current_point = self.find_min_f_point()
            # 从open_list中找到f值最小的节点
            self.open_list.remove(current_point)
            # 将该节点加入close_list
            self.close_list.append(current_point)
            # 判断当前节点是否为终点
            # print(current_point[0:2])
            if current_point[0:2] == self.target:
                # print('find target')
                break
            # 找到当前节点的邻居节点
            neighbors = []
            for i in range(-1, 2):
                for j in range(-1, 2):
                    if i == 0 and j == 0:
                        continue
                    neighbor = [current_point[0]+i, current_point[1]+j]
                    if self.is_in_map(neighbor) and not self.is_in_obstacle(neighbor):
                        neighbors.append(neighbor)
            # 计算邻居节点的f,g,h值
            for i in range(len(neighbors)):
                if self.is_in_close_list(neighbors[i]):
                    continue
                if not self.is_in_open_list(neighbors[i]):
                    self.open_list.append(neighbors[i])
                    neighbors[i].append(0)
                    neighbors[i].append(0)
                    neighbors[i].append(0)
                # 计算g值
                neighbors[i][3] = current_point[3]+1 
                # 计算h值
                neighbors[i][4] = func(neighbors[i])
                # 计算f值
                neighbors[i][2] = neighbors[i][3]+neighbors[i][4]
                # 将当前节点作为邻居节点的父节点
                neighbors[i].append((current_point[0], current_point[1]))
                # print(neighbors[i])
        # 从tartget开始回溯路径
        current_point = (self.target[0], self.target[1])
        while current_point != (self.start[0], self.start[1]):
            for i in range(len(self.close_list)):
                if current_point == (self.close_list[i][0], self.close_list[i][1]):
                    self.path.append(current_point)
                    current_point = self.close_list[i][5]
        self.path.append((self.start[0], self.start[1]))
        self.path.reverse()
        return self.path

if __name__ == '__main__':
    rospy.init_node('path_planning')
    occupancy_grid_reader = mr.OccupancyGridReader()
    start = occupancy_grid_reader.get_uav_grid_position()
    target = [80,90]
    target_ = occupancy_grid_reader.grid_to_real(target)
    print(target_)
    map = occupancy_grid_reader.currtent_map
    a_star = AStar(start=start, target=target, map=map)
    rate = rospy.Rate(10)
    print(map.data[0])
    print(a_star.get_path(a_star.heuristic_Manhattan))